

#include "stdafx.h"
#include "stdio.h"
#include "ColorSpaceConverter.h"
#include <string.h>

FILE* fp = fopen("test.yuv", "w+b");

#define clip(x)         (x > 255)? 255 : (x < 0)? 0 : x;

ColorSpaceConverter::ColorSpaceConverter (int Width, int Height) 
{
    width = Width;
    height = Height;
    frame422 = new unsigned char[width * height * 3 * 2];
    frame444 = frame422 + width * height * 3;
}

ColorSpaceConverter::~ColorSpaceConverter ()
{
    if (frame422 != NULL) {
        frame444 = NULL;
        delete [] frame422;
        frame422 = NULL;
    }
}

void ColorSpaceConverter::Convert422To444 (unsigned char* source,
                                            unsigned char* dest)
{
    int frameSize =  width * height;
    int chrWidth = width >> 1;
    int chrHeight = height >> 1;
    unsigned char* srcU = source + frameSize;
    unsigned char* srcV = srcU   + frameSize / 2;

    unsigned char* dstU = dest + frameSize;
    unsigned char* dstV = dstU + frameSize;

    for (int i = 0; i < height; i++) {
        unsigned char* curSrcU = srcU  + i * chrWidth;
        unsigned char* curDstU = dstU  + i * width;
        unsigned char* curSrcV = srcV  + i * chrWidth;
        unsigned char* curDstV = dstV  + i * width;

        for (int j = 0; j < chrWidth; j++) {
            *curDstU++ = *curSrcU;
            *curDstU = *(curDstU - 1);
            curDstU++;
            curSrcU++;

            *curDstV++ = *curSrcV;
            *curDstV = *(curDstV - 1);
            curDstV++;
            curSrcV++;
        }
    }

    memcpy(dest, source, frameSize);


}


void ColorSpaceConverter::Convert420TO422 (unsigned char* source,
                                            unsigned char* dest)
{
    int frameSize =  width * height;
    int chrWidth = width >> 1;
    int chrHeight = height >> 1;
    unsigned char* srcU = source + frameSize;
    unsigned char* srcV = srcU   + frameSize / 4;

    unsigned char* dstU = dest + frameSize;
    unsigned char* dstV = dstU + frameSize / 2;

    for (int i = 0; i < chrWidth; i++) {
        unsigned char* curSrcU = srcU  + i;
        unsigned char* curDstU = dstU  + i;
        unsigned char* curSrcV = srcV  + i;
        unsigned char* curDstV = dstV  + i;

        for (int j = 0; j < chrHeight; j++) {
            *curDstU = *curSrcU;
            curDstU += chrWidth;
            *curDstU = *curSrcU;
            curDstU += chrWidth;
            curSrcU += chrWidth;

            *curDstV = *curSrcV;
            curDstV += chrWidth;
            *curDstV = *curSrcV;
            curDstV += chrWidth;
            curSrcV += chrWidth;
        }
    }

    memcpy(dest, source, frameSize);


}


void ColorSpaceConverter::Convert444ToRGB (unsigned char* source,
                                           unsigned char* dest)
{
    int frameSize =  width * height;
    unsigned char* destTot = dest;
    unsigned char* srcY    = source;
    unsigned char* srcU    = srcY + frameSize;
    unsigned char* srcV    = srcU + frameSize;

    for (int i = 0; i < height; i++) {
        unsigned char* srcYt    = srcY + i * width;
        unsigned char* srcUt    = srcU + i * width;
        unsigned char* srcVt    = srcV + i * width;

        int C = 0, D = 0, E = 0;
        for (int j = 0; j < width; j++) {

            C = *srcYt++ - 16;
            D = *srcUt++ - 128;
            E = *srcVt++ - 128;


            *destTot++ = clip(( 298 * C + 516 * D           + 128) >> 8); // B
            *destTot++ = clip(( 298 * C - 100 * D - 208 * E + 128) >> 8); // G
            *destTot++ = clip(( 298 * C           + 409 * E + 128) >> 8); // R

        }
    }
}


void ColorSpaceConverter::Convert420ToRGB (unsigned char* source,
                                           unsigned char* dest)
{
    memset(frame422, 0, width * height * 3);
    memset(frame444, 0, width * height * 3);

    if (frame422 != NULL && frame444 != NULL) {
        Convert420TO422(source, frame422); 
        Convert422To444(frame422, frame444);
        Convert444ToRGB(frame444, dest);
    if (fp != NULL) {
        fwrite(dest, 1, width * height * 3, fp);
        fclose(fp);
    }
    }
}